#!  /usr/bin/env python
# coding=utf-8

import rospy
import numpy as np
import matplotlib.pyplot as plt
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
import math
import time
import json

from quartic_scurve import QUARTIC_SCURVE
from ti5_fkik_24 import ti5SimpleIK_p2j, ti5_j2p

from ti5arm.msg import ti5_polyJC_Lists


pi = math.pi

# mpl.rcParams['font.sans-serif'] = ['SimHei']  # 指定默认字体
mpl.rcParams['axes.unicode_minus'] = False  # 解决保存图像是负号'-'显示为方块的问题


class PLOT_ARC:
    def __init__(self, pc, p1, p2, r, c_angle):
        """通过圆心和两端点绘制圆弧

        Args:
            pc (np.ndarray((3,),np.float)): 圆心
            p1 (np.ndarray((3,),np.float)): 端点
            p2 (np.ndarray((3,),np.float)): 端点
            r (float): 半径
            c_angle (float): 圆心角
        """
        
        self.pc = pc
        self.p1 = p1
        self.p2 = p2
        self.r = r
        
        # 平面参数
        self.plane_A = 0.0
        self.plane_B = 0.0
        self.plane_C = 0.0
        self.plane_D = 0.0
        
        # 平面单位法向量
        self.normal_vector = np.zeros(3) 
        
     
        # 平面的三个坐标轴
        self.x = np.zeros(3)
        self.y = np.zeros(3)
        self.z = np.zeros(3)
        
        # 圆心角
        self.center_angle = c_angle
        # 弧长
        self.arc_length = self.center_angle*self.r
        
        # 由世界坐标到平面坐标的变换矩阵
        self.rM = np.eye(3)
        self.tM = np.eye(4)
         
        # 圆心到弦中点的距离
        self.length_pc_p1p2 = self.cal_v_die_length(0.5*(self.p2+self.p1) - self.pc )
        
        # 两端点的距离
        self.length_p1_p2 = self.cal_v_die_length(self.p2-self.p1)
        
        
        
        pass
    
    def cal_v_die_length(self, v):
        """计算输入向量的模长

        Args:
            v (_type_): 输入的向量 np.ndarray(np.float)

        Returns:
            _type_: 模长 float
        """
        return math.sqrt(np.sum(v*v))
        pass    
       
    def cal_2vector_angle(self, v1, v2):
        """计算两向量夹角
        """
        
        angle = np.sum(v1*v2)/math.sqrt(np.sum(v1*v1)*np.sum(v2*v2))
        return angle
        pass
    
    def cal_cross(self, v1,v2):
        """计算向量叉乘

        Args:
            v1 (_type_): _description_
            v2 (_type_): _description_
        """
        
        
        return np.array([v1[1]*v2[2] - v1[2]*v2[1], 
                           v1[2]*v2[0] - v1[0]*v2[2], 
                           v1[0]*v2[1] - v1[1]*v2[0] ])
        
        pass
        
    def cal_plane_param(self):
        x1 = self.p1[0]; y1 = self.p1[1]; z1 = self.p1[2]
        x2 = self.p2[0]; y2 = self.p2[1]; z2 = self.p2[2]
        x3 = self.pc[0]; y3 = self.pc[1]; z3 = self.pc[2]

    
        self.plane_A = (y2-y1)*(z3-z1) - (z2-z1)*(y3-y1)
        self.plane_B = (x3-x1)*(z2-z1) - (x2-x1)*(z3-z1)
        self.plane_C = (x2-x1)*(y3-y1) - (x3-x1)*(y2-y1)

        self.plane_D = -(self.plane_A*x1 + self.plane_B*y1 +self.plane_C*z1)
        
        v = np.array([self.plane_A,self.plane_B,self.plane_C])
        
        self.normal_vector = v/(math.sqrt(np.sum(v*v))) # 平面单位法向量
       
        
        pass
    
    def cal_plane_axis(self):
        
        self.x = (self.p2 - self.p1) / self.cal_v_die_length(self.p2-self.p1)
        
        self.y =  (0.5*(self.p1+self.p2) - self.pc) / self.cal_v_die_length(0.5*(self.p1+self.p2) - self.pc)
        
        
        self.z = np.array([self.x[1]*self.y[2] - self.x[2]*self.y[1], 
                           self.x[2]*self.y[0] - self.x[0]*self.y[2], 
                           self.x[0]*self.y[1] - self.x[1]*self.y[0] ])
        
        
        pass
    
    def cal_relative_p1p2(self):
        """计算起点和终点在新坐标的位置
        """
        self.relative_p1 = np.zeros(3)
        self.relative_p2 = np.zeros(3)
        tmp_angle = (pi - self.center_angle)/2
        
        self.relative_p1[0] =  -self.r * math.cos(tmp_angle)
        self.relative_p1[1] =  self.r * math.sin(tmp_angle)
        
        self.relative_p2[0] =  self.r * math.cos(tmp_angle)
        self.relative_p2[1] =  self.r * math.sin(tmp_angle)
        
        pass
    
    def cal_rotM(self):
        
        """
            ## 待求坐标相对于世界坐标的旋转矩阵每列表示，待求坐标系在世界坐标系上的投影 
            #### 即第1列分别为 待求x轴在世界坐标XYZ上的投影
            #### 即第2列分别为 待求y轴在世界坐标XYZ上的投影
            #### 即第3列分别为 待求z轴在世界坐标XYZ上的投影
        """
        self.rM = np.eye(3)

        self.rM[0,0] = self.x[0]
        self.rM[1,0] = self.x[1]
        self.rM[2,0] = self.x[2]
        self.rM[0,1] = self.y[0]
        self.rM[1,1] = self.y[1]
        self.rM[2,1] = self.y[2]
        self.rM[0,2] = self.z[0]
        self.rM[1,2] = self.z[1]
        self.rM[2,2] = self.z[2]
        
        pass
    
    def cal_tM(self):
        
        self.tM = np.eye(4)
        
        self.tM[0:3,0:3] = self.rM
        self.tM[0,3] = self.pc[0]
        self.tM[1,3] = self.pc[1]
        self.tM[2,3] = self.pc[2]
        
        pass
    
    def updata_arc_param(self):
        
        self.cal_plane_param()
        self.cal_plane_axis()
        self.cal_rotM()
        self.cal_tM()
        pass
    
    def cal_arc_point(self, n):
        """通过坐标变换插补圆弧

        Args:
            n (_type_): _description_

        Returns:
            _type_: _description_
        """
        
        x = np.linspace(-self.length_p1_p2/2,self.length_p1_p2/2, n)
        
        y = np.sqrt(self.r*self.r - x*x)
        
        z = np.zeros(n)
        
        # plt.figure()
        
        # plt.plot(x,y)
        
        # plt.xlim(-self.r, self.r)
        # plt.ylim(-self.r, self.r)
        
        # plt.show()
        
        data_arc = np.ones((n,4))
        res = np.ones((n,4))
        data_arc[:,0] = x
        data_arc[:,1] = y
        data_arc[:,2] = z
        
        for i in range(0,n):
            
            res[i,:] = np.squeeze(np.dot(self.tM, np.reshape(data_arc[i,],(4,1))))
            # res[i,0] = self.rM[0,0]*x[i] + self.rM[0,1]*y[i] + self.rM[0,2]*z[i]
            # res[i,1] = self.rM[1,0]*x[i] + self.rM[1,1]*y[i] + self.rM[1,2]*z[i]
            # res[i,2] = self.rM[2,0]*x[i] + self.rM[2,1]*y[i] + self.rM[2,2]*z[i]
        # res = res + self.tM[0:3,3]
        
        return res
        pass
    
    def cal_arc_point_v(self, n):
        """采用向量插补圆弧，无需坐标变换

        Args:
            n (_type_): _description_

        Returns:
            _type_: _description_
        """
        
        x = np.linspace(-self.length_p1_p2/2,self.length_p1_p2/2, n)
        
        y = np.sqrt(self.r*self.r - x*x)
        
    
        res = np.ones((n,3))
        for i in range(0,n):
            res[i,:] = x[i]*self.x + y[i]*self.y + self.pc
            pass

    
        return res
        pass
     
    
    def plot_opt_arc(self,data,p1,p2,p3,pt1,pt2):
        
        
        fig = plt.figure()
        ax = fig.gca(projection='3d')
        ax.plot(data[:,0],data[:,1],data[:,2], label='arc')
        
        
        
        
        data1 = np.zeros((1000,3))
        
        data1[0:500,0] = np.linspace(p1[0],p2[0],500)
        data1[0:500,1] = np.linspace(p1[1],p2[1],500)
        data1[0:500,2] = np.linspace(p1[2],p2[2],500)
        data1[500:1000,0] = np.linspace(p2[0],p3[0],500)
        data1[500:1000,1] = np.linspace(p2[1],p3[1],500)
        data1[500:1000,2] = np.linspace(p2[2],p3[2],500)
        
        
        
        # ax = fig.gca(projection='3d')
        ax.plot(data1[:,0],data1[:,1],data1[:,2], label='path curve')
        v_pt1_pt2 = pt2 - pt1
        die_length_pt1_pt2 = self.cal_v_die_length(v_pt1_pt2)
        data2 = np.zeros((100,3))
        data2[:,0] = np.linspace(-0.5*die_length_pt1_pt2,0.5*die_length_pt1_pt2,100)
        data2[:,1] = self.cal_v_die_length(pt1-self.pc + pt2 - self.pc)*0.5
        
        for i in range(0,100):
            data2[i,:] = np.squeeze(np.dot(self.rM, np.reshape(data2[i,:], (3,1))) + np.reshape(self.tM[0:3,3], (3,1)) )
        
   
        ax.plot(data2[:,0],data2[:,1],data2[:,2], label='arc string')
        
        ax.legend()
        plt.show()
        
        pass
    
    def plot_opt_grab_arc(self,arcdata1,arcdata2,p1,pt1,pt2,pt3,pt4,p4):
        
        if (self.cal_cross(pt2-p1, pt4-p4)==np.zeros(3)).all():
            pt1, pt2 = pt2, pt1
        if (self.cal_cross(pt1-p1, pt3-p4)==np.zeros(3)).all():
            pt3, pt4 = pt4, pt3
        
        
        line_data = np.zeros((1500,3))
        
        line_data[0:500,0] = np.linspace(p1[0],pt1[0],500)
        line_data[0:500,1] = np.linspace(p1[1],pt1[1],500)
        line_data[0:500,2] = np.linspace(p1[2],pt1[2],500)
        line_data[500:1000,0] = np.linspace(pt2[0],pt3[0],500)
        line_data[500:1000,1] = np.linspace(pt2[1],pt3[1],500)
        line_data[500:1000,2] = np.linspace(pt2[2],pt3[2],500)
        line_data[1000:1500,0] = np.linspace(pt4[0],p4[0],500)
        line_data[1000:1500,1] = np.linspace(pt4[1],p4[1],500)
        line_data[1000:1500,2] = np.linspace(pt4[2],p4[2],500)
        
        
        f = plt.figure()
        fig = f.add_subplot(projection='3d')
        
        
        fig.plot(line_data[0:500,0],line_data[0:500,1],line_data[0:500,2], color='b', label='原始路径')
        fig.plot(line_data[500:1000,0],line_data[500:1000,1],line_data[500:1000,2], color='b')
        fig.plot(line_data[1000:1500,0],line_data[1000:1500,1],line_data[1000:1500,2], color='b')
        
        
        # arcdata = np.concatenate((arcdata1, arcdata2), axis = 0)
        fig.plot(arcdata1[:,0],arcdata1[:,1],arcdata1[:,2], color = 'r' ,  label='过渡圆弧')
        fig.plot(arcdata2[:,0],arcdata2[:,1],arcdata2[:,2], color = 'r'  )
        
        
        
        fig.legend(loc = 'best')
        
        
        fig.tick_params(axis="x", direction="in")  # x轴刻度线向内
        fig.tick_params(axis="y", direction="in")  # y轴刻度线向内
        fig.set_xlabel(r'$x$', fontdict = {'size':15, 'color':'black'})
        fig.set_ylabel(r'$y$', fontdict = {'size':15, 'color':'black'})
        fig.set_zlabel(r'$z$', fontdict = {'size':15, 'color':'black'})
        
        
        plt.grid(True)
        
        plt.show()
        
        pass
    
    pass


class PATH_ARC:
    def __init__(self, p1, p2, p3, k, epuxitong = 0.1, Amax=300, T=0.01):
        """对直线段组成的路径的衔接点进行圆弧过渡

        Args:
            p1 (np.ndarray((3,), np.float)): 起点坐标
            p2 (np.ndarray((3,), np.float)): 衔接点坐标
            p3 (np.ndarray((3,), np.float)): 终点坐标
            k (float): 过渡圆弧半径调节系数
            epuxitong：最大弓高误差
            Amax：加速度约束
            T:轨迹插补周期
        """

        # input
        self.k = k # 半径调节系数
        self.p1 = p1 # 原路径端点坐标
        self.p2 = p2
        self.p3 = p3
        self.epuxitong = epuxitong # 最大弓高误差
        self.Amax = Amax # 加速度约束
        self.T = T # 轨迹插补周期 
        
        
        self.v_p2p1 = self.p1-self.p2 # 向量p2->p1
        self.v_p2p3 = self.p3-self.p2
        
        # 两段轨迹夹角
        self.theta = 0.0
        
        
        self.die_length_p2p1 = self.cal_v_die_length(self.v_p2p1) # 向量模长
        self.die_length_p2p3 = self.cal_v_die_length(self.v_p2p3)
        
        self.v_pm = np.zeros(3)
        self.die_length_pm = 0.0
        self.die_length_pc = 0.0
        
        self.v_pc = np.zeros(3)
        self.v_p2pt1 = np.zeros(3)
        self.v_p2pt2 = np.zeros(3)
        self.die_length_p2pt1 = 0.0
        self.die_length_p2pt2 = 0.0
        
        
        # output
        self.r = 0.0 # 代求过渡圆弧半径
        self.c = np.zeros(3) # 圆弧运圆心坐标
        self.pt1 = np.zeros(3) # 圆弧衔接点坐标
        self.pt2 = np.zeros(3)
        self.d1 = 0.0 # 过渡后路径段1长度
        self.d2 = 0.0 # 过渡后路径段2长度（圆弧长度）
        self.d3 = 0.0 # 过渡后路径段3长度
        self.arc_v_limit = 0.0 # 圆弧段速度约束
        self.center_angle = 0.0 # 过渡圆弧圆心角
        pass

    def cal_v_die_length(self, v):
        """计算输入向量的模长

        Args:
            v (_type_): 输入的向量 np.ndarray(np.float)

        Returns:
            _type_: 模长 float
        """
        return math.sqrt(np.sum(v*v))
        pass    
        
    def cal_theta(self):
        """计算过渡圆心角
        """
        
        self.theta = np.arccos(np.sum(self.v_p2p1*self.v_p2p3)/(self.die_length_p2p1*self.die_length_p2p3))
        self.center_angle = pi - self.theta
        pass

    def cal_r(self):
        """计算圆弧半径
        """
        self.r = self.k*self.die_length_p2p1*np.tan(self.theta/2) if (self.die_length_p2p1 <= self.die_length_p2p3)  \
            else self.k*self.die_length_p2p3*np.tan(self.theta/2)
            
        pass

    def cal_C(self):
        """计算圆心坐标
        """
        self.v_p2pt1 = (self.r*self.v_p2p1)/(np.tan(self.theta/2)*self.die_length_p2p1)
        self.v_p2pt2 = (self.r*self.v_p2p3)/(np.tan(self.theta/2)*self.die_length_p2p3)
        self.die_length_p2pt1 = self.cal_v_die_length(self.v_p2pt1)
        self.die_length_p2pt2 = self.cal_v_die_length(self.v_p2pt2)

        self.v_pm = 0.5*(self.v_p2pt1+self.v_p2pt2)
        self.die_length_pm = self.cal_v_die_length(self.v_pm)
    
        self.die_length_pc = math.sqrt(self.die_length_p2pt1*self.die_length_p2pt1 + self.r*self.r)
        # if self.die_length_pc!=math.sqrt(self.die_length_p2pt2*self.die_length_p2pt2 + self.r*self.r):
        #     print('self.die_length_pc err! ')
        self.v_pc = self.v_pm*self.die_length_pc/self.die_length_pm
        
        self.c = self.p2 + self.v_pc
        self.pt1 = self.p2 + self.v_p2pt1
        self.pt2 = self.p2 + self.v_p2pt2
        
        
        # if  self.cal_v_die_length(self.pt1-self.v_pc) != self.cal_v_die_length(self.pt2-self.v_pc):
        #     print('r or pt1 or pt2 or c err!')
        
        pass
     
    def cal_path_length(self):
        """计算路径段长度
        """
        self.d1 = self.die_length_p2p1 - self.die_length_p2pt1
        self.d2 = self.center_angle*self.r
        self.d3 = self.die_length_p2p3 - self.die_length_p2pt2
        
        pass    

    def cal_vlimit(self):
        """计算圆弧段速度限制
        """
        
        self.arc_v_limit = np.min( np.array( [math.sqrt(self.Amax*self.r), 
                                              2*self.r*np.arccos((self.r-self.epuxitong)/self.r)/self.T ] ) )
        
        pass


    def updata_parm(self, plot=1):
        """计算过渡圆弧参数
        """
        
        self.cal_theta()
        self.cal_r()
        self.cal_C()
        self.cal_path_length()
        self.cal_vlimit()
        
        if plot:
            self.plot_ori_path()
        
        pass

    def plot_ori_path(self):
        """绘制原始路径
        """
        
        data = np.zeros((1000,3))
        
        data[0:500,0] = np.linspace(self.p1[0],self.p2[0],500)
        data[0:500,1] = np.linspace(self.p1[1],self.p2[1],500)
        data[0:500,2] = np.linspace(self.p1[2],self.p2[2],500)
        data[500:1000,0] = np.linspace(self.p2[0],self.p3[0],500)
        data[500:1000,1] = np.linspace(self.p2[1],self.p3[1],500)
        data[500:1000,2] = np.linspace(self.p2[2],self.p3[2],500)

        
        fig = plt.figure()
        ax = fig.gca(projection='3d')
        ax.plot(data[:,0],data[:,1],data[:,2], label='path curve')
        ax.legend()
        
        plt.show()
        
        pass

    def plot_opt_path(self):
        """绘制圆弧处理后的路径
        """
        
        
        
        pass  
    
    
      
    pass


class PROCESS_GRAB_PATH:
    
    def __init__(self, p1,p2,p3,p4,k = 0.1 ,epuxitong=0.1, Amax = 300, T = 0.01, interpoly_num = 1000) :
        """处理抓取路径

        Args:
            p1 (np.ndarray((3,), np.float)): 起点坐标
            p2 (np.ndarray((3,), np.float)): 衔接点坐标
            p3 (np.ndarray((3,), np.float)): 衔接点坐标
            p4 (np.ndarray((3,), np.float)): 终点坐标
            k (float, optional): 过渡圆弧半径调节系数. Defaults to 0.1.
            epuxitong (float, optional): 最大弓高误差. Defaults to 0.1.
            Amax (int, optional): 加速度约束. Defaults to 300.
            T (float, optional): 轨迹插补周期. Defaults to 0.01.
            interpoly_num (int, optional): 插补点个数. Defaults to 1000.
        """
        
        self.p1 = p1
        self.p2 = p2
        self.p3 = p3
        self.p4 = p4
        self.k = k
        self.epuxitong = epuxitong
        self.Amax = Amax
        self.T = T
        self.interpoly_num = interpoly_num
        
        self.pa1 = PATH_ARC(p1 = p1, 
                    p2 = p2, 
                    p3 = p3,
                    k = k ,
                    epuxitong=epuxitong,
                    Amax = Amax,
                    T = T   )
        self.pa1.updata_parm(plot=0)
        self.plot_arc1 = PLOT_ARC(self.pa1.c, self.pa1.pt1, self.pa1.pt2, self.pa1.cal_v_die_length(self.pa1.pt2-self.pa1.c), self.pa1.center_angle)
        self.plot_arc1.updata_arc_param()
        self.arcdata1 = self.plot_arc1.cal_arc_point(100)
        

        self.pa2 = PATH_ARC(p1 = p2, 
                    p2 = p3, 
                    p3 = p4,
                    k = k ,
                    epuxitong=epuxitong,
                    Amax = Amax,
                    T = T   )
        self.pa2.updata_parm(plot=0)
        self.plot_arc2 = PLOT_ARC(self.pa2.c, self.pa2.pt1, self.pa2.pt2, self.pa2.cal_v_die_length(self.pa2.pt2-self.pa2.c), self.pa2.center_angle)
        self.plot_arc2.updata_arc_param()
        self.arcdata2 = self.plot_arc2.cal_arc_point(100)
        
        # 总路径长度
        
        self.path_length1 = self.pa1.d1
        self.path_length2 = self.pa1.d2
        self.path_length3 = self.pa1.d3 - self.pa2.r
        self.path_length4 = self.pa2.d2
        self.path_length5 = self.pa2.d3
        self.path_length = self.path_length1 + self.path_length2 + self.path_length3 + self.path_length4 + self.path_length5
        
        
        
        
        pass
    
    def path_xyz(self, x):
        
        def arc_interpoly(arc,x):
            theta =   x/arc.r
    
            r_p = np.zeros(3)
            if theta <= arc.center_angle/2:
                tmp_theta = (pi-arc.center_angle)/2 + theta
                r_p[0] = -arc.r*math.cos(tmp_theta)
                r_p[1] = arc.r*math.sin(tmp_theta)
            else:
                tmp_theta = (pi-arc.center_angle)/2 + arc.center_angle-theta
                r_p[0] = arc.r*math.cos(tmp_theta)
                r_p[1] = arc.r*math.sin(tmp_theta)
                pass
            return r_p[0]*arc.x + r_p[1]*arc.y + arc.pc
                
            pass
        
        if x <=self.path_length1:
            return self.p1 + (self.pa1.pt1 - self.p1) * x/self.path_length1
            pass
        if x <=self.path_length2 + self.path_length1:
            return arc_interpoly(self.plot_arc1,x-self.path_length1)
            pass
        if x <=self.path_length3 + self.path_length2 + self.path_length1:
            return self.pa1.pt2 + (self.pa2.pt1 - self.pa1.pt2) * (x-self.path_length1-self.path_length2)/self.path_length3
            pass
        if x <=self.path_length4 + self.path_length3 + self.path_length2 + self.path_length1:
            return arc_interpoly(self.plot_arc2,x-self.path_length1-self.path_length2-self.path_length3)
            pass
        if x <=self.path_length:
            return self.pa2.pt2 + (self.p4 - self.pa2.pt2) * (x-self.path_length1-self.path_length2-self.path_length3-self.path_length4)/self.path_length5
            pass
        else:
            print("input x err in path_xyz()")
            return self.p1
        
        pass

    def get_grab_path(self, interpoly_num=1000, s = None, cmdRate = None):
        """_summary_

        Args:
            interpoly_num (int, optional): _description_. Defaults to 1000.
            s (str, optional): interpoly method. Defaults to None. 

        Returns:
            _type_: _description_
            
        Eg:
            self.get_grab_path(interpoly_num=1000, s = 'quartic')
        """
        
        x = np.zeros(interpoly_num)
        
        if s=='quartic':
            self.q = QUARTIC_SCURVE(self.path_length, 100, 300, 900, 9000)
            self.q.cal_Param()
            tmp = np.linspace(0,self.q.Tcost,int(cmdRate*self.q.Tcost)) if cmdRate!=None else np.linspace(0,self.q.Tcost,int(interpoly_num))
            x = np.vectorize(self.q.Dis)(tmp)
            self.Tcost = self.q.Tcost
        else:
            x = np.linspace(0,self.path_length,num=interpoly_num)

        interpoly_num = x.shape[0]

        self.grab_path_xyz = np.zeros((interpoly_num,3))
        for i in range(0,interpoly_num):
            self.grab_path_xyz[i,:] = self.path_xyz(x[i])
            pass
        return self.grab_path_xyz
        
        pass

    def plot_grab_path(self, interpoly_num=1000):
        
        stime = time.time()
        
        try:
            path_xyz = self.grab_path_xyz
        except Exception as e:
            print("get_grab_path...")
            
            path_xyz = self.get_grab_path(interpoly_num)
            
            pass
        
        etime = time.time()
        use_time = (etime - stime)*1000.0
        print("plot_grab_path use_time:%.3fms"%(use_time))
        f = plt.figure()
        fig = f.gca(projection='3d')
        # fig = f.add_subplot(projection='3d')
        fig.plot(path_xyz[:,0], path_xyz[:,1], path_xyz[:,2])
        fig.tick_params(axis="x", direction="in")  # x轴刻度线向内
        fig.tick_params(axis="y", direction="in")  # y轴刻度线向内
        fig.set_xlabel(r'$x$', fontdict = {'size':15, 'color':'black'})
        fig.set_ylabel(r'$y$', fontdict = {'size':15, 'color':'black'})
        fig.set_zlabel(r'$z$', fontdict = {'size':15, 'color':'black'})

        start_point = tuple(self.p1) # 这是您要标注的点的坐标
        fig.scatter(*start_point, color='g', s=10)  # 在图中绘制红色的点
        fig.text(*start_point, s="start", color='g')  # 在点的位置添加文字标注
        end_point = tuple(self.p4) # 这是您要标注的点的坐标
        fig.scatter(*end_point, color='r', s=10)  # 在图中绘制红色的点
        fig.text(*end_point, s="end", color='r')  # 在点的位置添加文字标注


        plt.grid(True)
        
        print("arc_path_Vmax1:", self.pa1.arc_v_limit)
        print("arc_path_Vmax2:", self.pa2.arc_v_limit)
        print("path_xyz.shape:", path_xyz.shape)
        plt.show()
        
        
        try:
            print("try plot quarticCurve...")
            self.q.plot_quarticCurve(poly_num=interpoly_num)
        except Exception as e:
            print("no quarticCurve code exite")
            pass
        
        pass

    def get_arm_grab_jdeg(self, dir=None, interpoly_num=1000, pose=np.array([180,0,-180]), s=None, cmdRate = None):
        """_summary_

        Args:
            dir (_type_, optional): _description_. Defaults to None.
            interpoly_num (int, optional): _description_. Defaults to 1000.
            pose (_type_, optional): _description_. Defaults to np.array([180,0,-180]).
            s (_type_, optional): _description_. Defaults to None.

        Returns:
            _type_: grab_jdeg
            
        Eg:
            self.get_arm_grab_jdeg(dir='C:/Users/Hades/Desktop/paper/code/data_txt/path_arc_process_grabJdeg.txt',
                                   interpoly_num=1000,
                                   pose=np.array([180, 0, -180]),
                                   s='quartic')
        """
        stime = time.time()

        try:
            path_xyz = self.grab_path_xyz

            
        except Exception as e:
            print("get_grab_path...")
            
            path_xyz = self.get_grab_path(interpoly_num, s, cmdRate=cmdRate)
            
            pass
        interpoly_num = path_xyz.shape[0]


        tPose = np.concatenate((path_xyz, np.tile(pose,(interpoly_num,1))), axis=1)
        joint_deg = np.zeros((interpoly_num,6),dtype=np.float)
        for i in range(0,interpoly_num):
            joint_deg[i,:] = ti5SimpleIK_p2j(cJdeg=np.array([0,0,0,0,0,0]),
                                          tPose=tPose[i,:])
                                          
            pass
        etime = time.time() 
        print("get_arm_grab_jdeg use_time: %.3d ms"%((etime-stime)*1000.0))

        if dir==None:
            return joint_deg
        try :
            groups_jdeg = joint_deg.shape[0]
            with open(dir, mode='w') as file:
                if 1:
                    head = {'name':'Jdeg', 'writeTime':time.time(), 
                            'seq':'Jdeg','shape':[joint_deg.shape[0],joint_deg.shape[1]]}
                msg_head = json.dumps(head)
                file.write(msg_head)
            
                for i in range(0, groups_jdeg):
                    w_msg = "\n{}, {}, {}, {}, {}, {}".format(  joint_deg[i,0],
                                                                joint_deg[i,1],
                                                                joint_deg[i,2],
                                                                joint_deg[i,3],
                                                                joint_deg[i,4],
                                                                joint_deg[i,5]
                                                                )
                    file.write(w_msg)
                    pass
            print('arm_grab_jdeg write to {}'.format(dir))
        except Exception as e:
            print("write arm_grab_jdeg to {} falied!".format(dir))
            print('err: ', e)
            
        return joint_deg
        pass

    pass


def arm_grab_path_test(pub=None, cmd_rate=100):

    pgp = PROCESS_GRAB_PATH(p1 = np.array([300,200,50]), 
                            p2 = np.array([450,400,200]), 
                            p3 = np.array([300,-200,300]),
                            p4 = np.array([200,-300,50]),
                            k = 0.3)
    
    grab_jdeg = pgp.get_arm_grab_jdeg(dir=None,
                                        interpoly_num=1000,
                                        pose=np.array([180, 0, -180]),
                                        s='quartic',
                                        cmdRate=cmd_rate)
    
    jointTarget = ti5_polyJC_Lists()
    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 6
    jointTarget.interpolyReq = 0 # external J space linear interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = int(cmd_rate)
    # **************************************************************************
    
    poly_step = grab_jdeg.shape[0]
    #jointxRadList is deg
 
    Tcost = poly_step*(1.0/cmd_rate)

    for i in range(0,poly_step):
        jointTarget.joint1RadList.append(grab_jdeg[i,0] )
        jointTarget.joint2RadList.append(grab_jdeg[i,1] )
        jointTarget.joint3RadList.append(grab_jdeg[i,2] )
        jointTarget.joint4RadList.append(grab_jdeg[i,3] )
        jointTarget.joint5RadList.append(grab_jdeg[i,4] )
        jointTarget.joint6RadList.append(grab_jdeg[i,5] )
   
    # pub.publish(jointTarget)

    spose = ti5_j2p(grab_jdeg[0,:])
    epose = ti5_j2p(grab_jdeg[-1,:])

    np.set_printoptions(formatter={'float':'{:.3f}'.format}, suppress=True)

    print("spose",spose)
    print("epose",epose)
    print("grab_jdeg.shape:",grab_jdeg.shape)
    print('cmd_rate',cmd_rate, 'cmd_time:%.3f s'%(Tcost))                

    pgp.plot_grab_path(interpoly_num=1000)


    pass


def plot_arc_test():
    
    pgp = PROCESS_GRAB_PATH(p1 = np.array([300,200,50]), 
                            p2 = np.array([450,400,200]), 
                            p3 = np.array([300,-200,300]),
                            p4 = np.array([200,-300,50]),
                            k = 0.3)
    # pgp.get_grab_path(interpoly_num=1000, s='quartic')
    
    pgp.get_arm_grab_jdeg(dir=None,
                          interpoly_num=1000,
                          pose=np.array([180, 0, -180]),
                          s='quartic',
                          cmdRate=100)
    pgp.plot_grab_path(interpoly_num=1000)



    # pgp = PROCESS_GRAB_PATH(p1 = np.array([450,200,0]), 
    #                         p2 = np.array([450,200,200]), 
    #                         p3 = np.array([450,-200,200]),
    #                         p4 = np.array([450,-200,0]),
    #                         k = 0.3)
  
    # pgp.plot_grab_path(interpoly_num=1000)
    
    
    pass



if  __name__== "__main__":
    rospy.init_node("path_arc_process_node")
 
    # plot_arc_test()
    

    arm_grab_path_test( cmd_rate=100)

    pass
